//#include <fstream>
#include <math.h>

#include "Eigen/Eigen"
#include "Eigen/SparseQR"

#include "Laplacians.h"

using namespace Eigen;

Eigen::MatrixXd step1_heat_flow(double _time, double _step, int _source, const Eigen::MatrixXd& _B, const Eigen::MatrixXd& _Q)  // 0 <= source <= _LBO.rows-1
{
    std::cout << "Step 1: Integrate the heat flow " << std::endl;

    int np = _B.rows();

    MatrixXd U0 = MatrixXd::Zero(np, 1);
    U0(_source, 0) = 1;

    //MatrixXd A = _B - _time * _Q;
    //FullPivHouseholderQR<MatrixXd> dec(A);
    //MatrixXd Ut = dec.solve(_B * U0);

    //std::cout << "Step 1: done \n\n" << std::endl;
    //return Ut;


	////Solve the linear equation (id-step.LBO)Ut = U0, or (B-step.Q)Ut=B.U0, or Ax=b
    MatrixXd A = _B - _step * _Q;

    FullPivHouseholderQR<MatrixXd> dec(A);
    //SparseQR< SparseMatrix<double>, AMDOrdering<int> > dec;
    //dec.compute(A.sparseView());    
    int ns = ceil(_time / _step);
    for (int i = 1; i <= ns; i++)
    {
        std::cout << "Step " << i << " : Solving linear equation" << std::endl;        
        MatrixXd Ut = dec.solve(_B * U0);
        U0 = Ut;
    }
    

    std::cout << "Step 1: done \n\n" << std::endl;

    return U0;
}


MatrixXd vector_polynomial_basic(dPoint _pt)
{
    double x = _pt.x();
    double y = _pt.y();
    double z = _pt.z();

    MatrixXd b(20, 1);
    b(0, 0) = 1;
    b(1, 0) = x;
    b(2, 0) = y;
    b(3, 0) = z;
    b(4, 0) = x * x;
    b(5, 0) = y * y;
    b(6, 0) = z * z;
    b(7, 0) = x * y;
    b(8, 0) = y * z;
    b(9, 0) = z * x;
    b(10, 0) = x * x * x;
    b(11, 0) = y * y * y;
    b(12, 0) = z * z * z;
    b(13, 0) = x * x * y;
    b(14, 0) = x * x * z;
    b(15, 0) = y * y * x;
    b(16, 0) = y * y * z;
    b(17, 0) = z * z * y;
    b(18, 0) = z * z * x;
    b(19, 0) = x * y * z;

    return b;
}

double gradx(dPoint _pt, MatrixXd _c)
{
    double x = _pt.x();
    double y = _pt.y();
    double z = _pt.z();

    double gx = _c(1, 0) + 2 * _c(4, 0) * x + _c(7, 0) * y + _c(9, 0) * z + 3 * _c(10, 0) * x * x + 2 * _c(13, 0) * x * y + 2 * _c(14, 0) * x * z + _c(15, 0) * y * y + _c(18, 0) * z * z + _c(19, 0) * y * z;

    return gx;
}

double gradxx(dPoint _pt, MatrixXd _c)
{
    double x = _pt.x();
    double y = _pt.y();
    double z = _pt.z();

    double gxx = 2 * _c(4, 0) + 6 * _c(10, 0) * x + 2 * _c(13, 0) * y + 2 * _c(14, 0) * z;

    return gxx;
}


double grady(dPoint _pt, MatrixXd _c)
{
    double x = _pt.x();
    double y = _pt.y();
    double z = _pt.z();

    double gy = _c(2, 0) + 2 * _c(5, 0) * y + _c(7, 0) * x + _c(8, 0) * z + 3 * _c(11, 0) * y * y + _c(13, 0) * x * x + 2 * _c(15, 0) * x * y + 2 * _c(16, 0) * y * z + _c(17, 0) * z * z + _c(19, 0) * x * z;

    return gy;
}

double gradyy(dPoint _pt, MatrixXd _c)
{
    double x = _pt.x();
    double y = _pt.y();
    double z = _pt.z();

    double gyy = 2 * _c(5, 0) + 6 * _c(11, 0) * y + 2 * _c(15, 0) * y + 2 * _c(16, 0) * z;

    return gyy;
}

double gradz(dPoint _pt, MatrixXd _c)
{
    double x = _pt.x();
    double y = _pt.y();
    double z = _pt.z();

    double gz = _c(3, 0) + 2 * _c(6, 0) * z + _c(8, 0) * y + _c(9, 0) * x + 3 * _c(12, 0) * z * z + _c(14, 0) * x * x + _c(16, 0) * y * y + 2 * _c(17, 0) * y * z + 2 * _c(18, 0) * z * x + _c(19, 0) * x * y;

    return gz;
}


double gradzz(dPoint _pt, MatrixXd _c)
{
    double x = _pt.x();
    double y = _pt.y();
    double z = _pt.z();

    double gzz = 2 * _c(6, 0) + 6 * _c(12, 0) * z + 2 * _c(17, 0) * y + 2 * _c(18, 0) * x;

    return gzz;
}

double gradxy(dPoint _pt, MatrixXd _c)
{
    double x = _pt.x();
    double y = _pt.y();
    double z = _pt.z();

    double gxy = _c(7, 0) + 2 * _c(13, 0) * x + 2 * _c(15, 0) * y + _c(19, 0) * z;

    return gxy;
}

double gradyx(dPoint _pt, MatrixXd _c)
{
    double x = _pt.x();
    double y = _pt.y();
    double z = _pt.z();

    double gyx = _c(7, 0) + 2 * _c(13, 0) * x + 2 * _c(15, 0) * y + _c(19, 0) * z;

    return gyx;
}

double gradyz(dPoint _pt, MatrixXd _c)
{
    double x = _pt.x();
    double y = _pt.y();
    double z = _pt.z();

    double gyz = _c(8, 0) + 2 * _c(16, 0) * y + 2 * _c(17, 0) * z + _c(19, 0) * x;

    return gyz;
}

double gradzy(dPoint _pt, MatrixXd _c)
{
    double x = _pt.x();
    double y = _pt.y();
    double z = _pt.z();

    double gzy = _c(8, 0) + 2 * _c(16, 0) * y + 2 * _c(17, 0) * z + _c(19, 0) * x;

    return gzy;
}

double gradzx(dPoint _pt, MatrixXd _c)
{
    double x = _pt.x();
    double y = _pt.y();
    double z = _pt.z();

    double gzx = _c(9, 0) + 2 * _c(14, 0) * x + 2 * _c(18, 0) * z + _c(19, 0) * y;

    return gzx;
}


vector<MatrixXd> step2_evaluate_gradient(PCloud& _pc, const Eigen::MatrixXd& _f, const vector<vector<int>>& _vector_neighbor_id, const vector<vector<dPoint>>& _vector_neighbor_d, double _h)
{
    std::cout << "Step 2: Evaluate the gradient " << std::endl;

    unsigned int np = _pc.get_cloud_size();

    MatrixXd grad = MatrixXd::Zero(np, 3);
    MatrixXd X = MatrixXd::Zero(np, 3);
    MatrixXd div = MatrixXd::Zero(np, 1);


    for (unsigned int i = 0; i < np; i++)
    {
        dPoint pt = _pc.get_point(i);

        double x = pt.x();
        double y = pt.y();
        double z = pt.z();

        vector<dPoint> neighbor_pts = _vector_neighbor_d[i];
        vector<int> neighbor_id = _vector_neighbor_id[i];
        int nb = neighbor_pts.size();

		//if (i == 0)
		//{
		//	std::ofstream file("point_0.txt");
		//	if (file.is_open())
		//	{
		//		for (int j=0; j<nb; j++)
		//		{
		//			file << "id=" << neighbor_id[j] << " f=" << _f(neighbor_id[j], 0)<< " x="<<  neighbor_pts[j].x() << " y=" << neighbor_pts[j].y() << " z=" << neighbor_pts[j].z()<< endl;		
		//		}
		//	}
		//	file.close();
		//}

        ////Find all unknown coefficients c of function f by LS method
        MatrixXd left_side = MatrixXd::Zero(20, 20);
        MatrixXd right_side = MatrixXd::Zero(20, 1);

        for (int j = 0; j < nb; j++)
        {
            dPoint nb_pt = neighbor_pts[j];

            dVector vec = nb_pt - pt;
            double wti = exp(-CGAL::to_double(vec * vec) / (_h * _h));

            MatrixXd bi = vector_polynomial_basic(nb_pt);

            //left_side += wti*bi * bi.transpose();
            //         right_side += wti*bi * _f(neighbor_id[j],0);

            left_side += bi * bi.transpose();
            right_side += bi * _f(neighbor_id[j], 0);
        }

        FullPivHouseholderQR<MatrixXd> dec(left_side);
        MatrixXd c = dec.solve(right_side);

        //MatrixXd c = left_side.inverse() * right_side;


        ////find grad of f at point
        grad(i, 0) = gradx(pt, c);
        grad(i, 1) = grady(pt, c);
        grad(i, 2) = gradz(pt, c);

        ////Find X and div.X
        double grad_length = sqrt(grad(i, 0) * grad(i, 0) + grad(i, 1) * grad(i, 1) + grad(i, 2) * grad(i, 2));
        if (grad_length != 0)
        {
            X(i, 0) = grad(i, 0) / grad_length;
            X(i, 1) = grad(i, 1) / grad_length;
            X(i, 2) = grad(i, 2) / grad_length;

            div(i, 0) = (gradxx(pt, c) + gradyy(pt, c) + gradzz(pt, c)) / grad_length - (gradxx(pt, c) * gradx(pt, c) * gradx(pt, c) + gradyy(pt, c) * grady(pt, c) * grady(pt, c) + gradzz(pt, c) * gradz(pt, c) * gradz(pt, c) + 2 * gradxy(pt, c) * gradx(pt, c) * grady(pt, c) + 2 * gradyz(pt, c) * grady(pt, c) * gradz(pt, c) + 2 * gradzx(pt, c) * gradz(pt, c) * gradx(pt, c)) / (grad_length * grad_length * grad_length);
        }
        else
        {
            X(i, 0) = 0;
            X(i, 1) = 0;
            X(i, 2) = 0;

            div(i, 0) = 0;
        }

    }
    vector<MatrixXd> res;
    res.push_back(grad);
    res.push_back(X);
    res.push_back(div);
    std::cout << "Step 2: Done \n" << std::endl;

    return res;
}

//Eigen::MatrixXd step3_Poison_equation(const Eigen::MatrixXd& _LBO, const Eigen::MatrixXd& _div)
MatrixXd step3_Poison_equation(const MatrixXd& _B, const MatrixXd& _Q, const MatrixXd& _div)
{
    std::cout << "Step 3: Solve Poisson equation \n" << std::endl;

    FullPivHouseholderQR<MatrixXd> dec(_Q);
    MatrixXd phi = dec.solve(_B * _div);

    //cout << phi << endl;

    std::cout << "Step 3: Done \n" << std::endl;

    return phi;
}

vector<double> GetJetColour(double v, double vmin, double vmax)
{
    vector<double> c;
    c.push_back(1);
    c.push_back(1);
    c.push_back(1); // white
    double dv;

    if (v < vmin)
    { v = vmin; }
    if (v > vmax)
    { v = vmax; }
    dv = vmax - vmin;

    if (v < (vmin + 0.25 * dv))
    {
        c[0] = 0;
        c[1] = 4 * (v - vmin) / dv;
    }
    else
        if (v < (vmin + 0.5 * dv))
        {
            c[0] = 0;
            c[2] = 1 + 4 * (vmin + 0.25 * dv - v) / dv;
        }
        else
            if (v < (vmin + 0.75 * dv))
            {
                c[0] = 4 * (v - vmin - 0.5 * dv) / dv;
                c[2] = 0;
            }
            else
            {
                c[1] = 1 + 4 * (vmin + 0.75 * dv - v) / dv;
                c[2] = 0;
            }

    return (c);
}


bool vector_mapping(PCloud _pc, const MatrixXd& _mat, string _filename)
{
    if (_pc.get_cloud_size() != _mat.rows())
    {
        std::cout << " Sizes of pointcloud and mapping matrix are difference \n" << std::endl;

        return false;
    }
    else
    {
        int np = _pc.get_cloud_size();

        double max = _mat.maxCoeff();
        double min = _mat.minCoeff();
        if (max == min)
        {
			std::ofstream file(_filename);
			if (file.is_open())
			{
				for (int i = 0; i < np; i++)
				{
					dPoint pt = _pc.get_point(i);
					file << "Vertex " << i + 1 << " " << pt.x() << " " << pt.y() << " " << pt.z() << " {rgb=(1 1 1) uv=(0 0)}" << endl;
				}

				int nf = _pc.faces.size();
				for (int i = 0; i < nf; i++)
				{
					file << "Face " << i + 1 << " " << _pc.faces[i][0] << " " << _pc.faces[i][1] << " " << _pc.faces[i][2] << endl;
				}
			}

			file.close();
        }
        else
        {
			std::ofstream file(_filename);
			if (file.is_open())
			{
				for (int i = 0; i < np; i++)
				{
					vector<double> rgb = GetJetColour(_mat(i,0), min, max);
					
					double u = (max - _mat(i,0))/(max - min);	
					double v = u;

					dPoint pt = _pc.get_point(i);
					file << "Vertex " << i + 1 << " " << pt.x() << " " << pt.y() << " " << pt.z() << " {rgb=(" << rgb[0] << " " << rgb[1] << " " << rgb[2] << ") uv=(" << u << " " << v << ")}" << endl;
				}

				int nf = _pc.faces.size();
				for (int i = 0; i < nf; i++)
				{
					file << "Face " << i + 1 << " " << _pc.faces[i][0] << " " << _pc.faces[i][1] << " " << _pc.faces[i][2] << endl;
				}
			}

			file.close();
        }
        
        return true;
    }
}
